#include "StdAfx.h"

#include "Tube.h"

namespace Math
{
    Tube::Tube(const std::pair<gtl::Vec3d, gtl::Vec3d> & pp1
        , const std::pair<gtl::Vec3d, gtl::Vec3d> & pp2, double width, double height)
    {
        double focalRadius = height / 2.0 - std::sqrt( height * height / 4.0 - width * width / 4.0);
        Init(pp1, pp2, focalRadius);
    }
    
    Tube::Tube(const std::pair<gtl::Vec3d, gtl::Vec3d> & pp1
        , const std::pair<gtl::Vec3d, gtl::Vec3d> & pp2, double fradius)
    {
        Init(pp1, pp2, fradius);
    }


    bool Tube::Inside(const gtl::Vec3d & pt) const
    {
        bool inside = plane1.isInHalfSpace(pt) && plane2.isInHalfSpace(pt);
        if (inside) 
        {
            gtl::Rayd tw(p11, p21 - p11);
            gtl::Rayd bw(p12, p22 - p12);
            
            gtl::Vec3d tpr = tw.project(pt);
            gtl::Vec3d bpr = bw.project(pt);

            double len = (tpr - bpr).length();
            double sum0 = len + 2 * fradius;
            double sum1 = (pt - tpr).length() + (pt - bpr).length();
            inside = sum0 >= sum1;
            
        }
        
        return inside; 
    }


    void Tube::Init(const std::pair<gtl::Vec3d, gtl::Vec3d> & pp1
        , const std::pair<gtl::Vec3d, gtl::Vec3d> & pp2, double focalRadius)
    {
        p11 = pp1.first;
        p12 = pp1.second;
        p21 = pp2.first;
        p22 = pp2.second;
        fradius = focalRadius;

        //init planes
        center1 = (p11 + p12) * 0.5;
        center2 = (p21 + p22) * 0.5;
        
        gtl::Vec3d v1 = (p12 - p11);
        gtl::Vec3d norm1 = v1.cross(center2 - p11).cross(v1);
        norm1.normalize();

        gtl::Vec3d v2 = (p22 - p21);
        gtl::Vec3d norm2 = v2.cross(center1 - p21).cross(v2);
        norm2.normalize();

        plane1 = gtl::Planed(norm1, center1);
        plane2 = gtl::Planed(norm2, center2);
    }

    gtl::Vec3d Tube::ConvertToGlobal(const gtl::Vec3d & localPos) const
    {
        return localPos;
    }
    gtl::Vec3d Tube::ConvertToLocal(const gtl::Vec3d & globalPos) const
    {
        return globalPos;
    }

} //namespace
